#include "ModelProcessorWinter.h"
#include "../RapidXML/rapidxml.hpp"
#include <cmath>

using std::cout;
using std::endl;

namespace MMM
{

ModelProcessorWinter::ModelProcessorWinter()
: ModelProcessor()
{
    height = 1.8f;
	mass = 75.0f;
	name = "Winter";
    handLength = -1.;
    handWidth = -1.;

}

bool ModelProcessorWinter::setup(float height, float mass, float handLength, float handWidth)
{
	this->height = height;
	this->mass = mass;
    this->handLength = handLength;
    this->handWidth = handWidth;
	return true;
}

bool ModelProcessorWinter::setupSegmentLength(const std::string &segmentName, float lengthM)
{
    customSegmentLengths[segmentName] = lengthM;
    return true;
}


MMM::ModelPtr ModelProcessorWinter::convertModel(ModelPtr input)
{
	if (!input)
		return input;

	ModelPtr res = input->clone();
	std::vector<ModelNodePtr> models = res->getModels();
    std::map<std::string, float>::iterator custSegIt;
	for (auto & model : models)
	{
		// scale model
        custSegIt = customSegmentLengths.find(model->name);
        if (custSegIt != customSegmentLengths.end())
        {
            float l = model->localTransformation.block(0, 3, 3, 1).norm();
            float scaling = 1.0f;
            if (l != 0.0f)
                scaling = 1.0f / l * custSegIt->second;
            model->localTransformation.block(0, 3, 3, 1) *= scaling;
            model->scaling = scaling;
            
            // scale mass, com and inertia accordingly
            model->segment.mass *= mass;
            model->segment.com *= scaling;
            model->segment.inertia *= pow(scaling,2) * mass;
        }
        else
        {
            model->localTransformation.block(0, 3, 3, 1) *= height;
            model->scaling = height;

            // scale mass, com and inertia accordingly
            model->segment.scaling = mass;

            model->segment.mass *= mass;
            model->segment.com *= height;
            model->segment.inertia *= pow(height,2) * mass;

        }

		// todo prismatic joints!
 	}
	res->setHeight(height);
	res->setMass(mass);
	return res;
}


bool ModelProcessorWinter::_setup( rapidxml::xml_node<char>* rootTag )
{
	float h = height;
	float w = mass;
	rapidxml::xml_node<>* node = rootTag;

	std::string nodeName = XML::toLowerCase(node->name());
	if (nodeName == "modelprocessorconfig")
	{
		rapidxml::xml_attribute<> *attr = node->first_attribute("type", 0, false);
		if (!attr)
		{
			MMM_ERROR << "xml Tag: missing attribute 'type'" << endl;
			return false;
		}
		std::string jn = attr->value();
		if (jn.empty())
		{
			MMM_ERROR << "xml tag: null type string" << endl;
			return false;
		}
		XML::toLowerCase(jn);
		if (jn != "winter")
		{
			MMM_ERROR << "xml type tag: Expecting Winter, got " << jn << endl;
			return false;
		}
		node = node->first_node();
	}
	else 
	{
		MMM_ERROR << "Expecting modelprocessorconfig tag, but got " << nodeName << endl;
		return false;
	}
	
	while (node)
	{
		nodeName = XML::toLowerCase(node->name());
		if (nodeName == "height")
		{
			h = XML::convertToFloat(node->value());
		}
        else if (nodeName == "mass" || nodeName == "weight")
        {
            w = XML::convertToFloat(node->value());
        }
        else if (nodeName == "segmentlength")
        {
            float newValue = XML::convertToFloat(node->value());
            float sc = XML::getUnitsScalingToMeter(node);
            newValue *= sc;
            std::string segName = XML::getStringByAttributeName(node, "name", false);
            this->customSegmentLengths[segName] = newValue;
        }
        else
		{
			MMM_ERROR << "Ignoring unknown XML tag " << nodeName << endl;
		}
		node = node->next_sibling();
	}
	this->height = h;
	this->mass = w;
	//cout << "MASS : " << this->mass << endl;
    return true;
}

std::string ModelProcessorWinter::toXML(int nrTabs)
{
	std::string tab = "\t";
	std::string tabs;
	for (int i = 0; i < nrTabs; i++)
		tabs += tab;

	std::stringstream res;

	res << tabs << "<ModelProcessorConfig type='Winter'>" << endl;
	res << tabs << tab << "<Height>" << height << "</Height>" << endl;
	res << tabs << tab << "<Mass>" << mass << "</Mass>" << endl;
    std::map<std::string, float>::iterator it = customSegmentLengths.begin();
    while (it != customSegmentLengths.end())
    {
        res << tabs << tab << "<SegmentLength name='" << it->first << "' units='m'>" << it->second << "</SegmentLength>" << endl;
        it++;
    }
	res << tabs << "</ModelProcessorConfig>" << endl;
	return res.str();
}

void ModelProcessorWinter::appendProcessorDataXML(RapidXMLWriterNodePtr processorNode) {
    processorNode->append_node("Height")->append_data_node(XML::toString(height));
    processorNode->append_node("Mass")->append_data_node(XML::toString(mass));
    for (auto & customSegmentLength : customSegmentLengths) {
        processorNode->append_node("SegmentLength")->append_attribute("name", customSegmentLength.first)->append_attribute("units", "m")->append_data_node(XML::toString(customSegmentLength.second));
    }

}


float ModelProcessorWinter::getMass()
{
	return mass;
}

float ModelProcessorWinter::getHeight()
{
	return height;
}


std::map<std::string, float> ModelProcessorWinter::getCustomSegmentLengths()
{
    return customSegmentLengths;
}
}
